function [Dupsilon, Dalpha, fB, omegaIBB] = navpar2imuout_inc(mode, in,  cfg, par,  cst) %#codegen
% 根据姿态、速度及位置信息逆推加表、陀螺理想输出 TODO: 改为system object、完善说明
%
% Input Arguments:
% # mode: 模式选择，0表示初始化，1表示POS逆推
% # in.CBL: 3*3，姿态矩阵
% # in.vN: 3*1，东北天速度，单位m/s
% # in.pos: 3*1，地理系下位置，分别为纬度、经度、高度，单位（rad，rad，m）
% # cfg.CLLType: 水平坐标系姿态更新形式，1：根据旋转矢量计算，2：根据已知位置矩阵计算
% # cfg.rotVecType: 等效旋转矢量更新形式，1：姿态矩阵→四元数→旋转矢量，2：直接计算
% # par.dt: 采样间隔
% # par.g: 重力加速度
% # par.useExtGravity:
% # cst:

% Output Arguments:
% # Dupsilon: 3*1，比速度增量，单位m/s
% # Dalpha: 3*1，角增量，单位rad
% # fB: 3*1，本周期起始时刻（上一周期结束时刻）的比力，单位m/s^2
% # omegaIBB: 3*1，本周期起始时刻（上一周期结束时刻）的角速度，单位rad/s
%
% References:
% #《应用导航算法工程基础》“导航参数逆推导航传感器输出”

persistent p_mid_POS

if isempty(p_mid_POS)
    p_mid_POS.CBL_1 = coder.nullcopy(NaN(3, 3));
    p_mid_POS.vN_1 = coder.nullcopy(NaN(3, 1));
    p_mid_POS.pos_1 = coder.nullcopy(NaN(3, 1));
    p_mid_POS.CBL_2 = coder.nullcopy(NaN(3, 3));
    p_mid_POS.vN_2 = coder.nullcopy(NaN(3, 1));
    p_mid_POS.pos_2 = coder.nullcopy(NaN(3, 1));
    p_mid_POS.Dupsilon_1 = coder.nullcopy(NaN(3, 1));
    p_mid_POS.Dalpha_1 = coder.nullcopy(NaN(3, 1));
    p_mid_POS.Dupsilon = coder.nullcopy(NaN(3, 1));
    p_mid_POS.Dalpha = coder.nullcopy(NaN(3, 1));
end

switch mode
    case int8(0)
        p_mid_POS.CBL_1 = in.CBL;
        p_mid_POS.vN_1 = in.vN;
        p_mid_POS.pos_1 = in.pos;
        p_mid_POS.CBL_2 = in.CBL;
        p_mid_POS.vN_2 = in.vN;
        p_mid_POS.pos_2 = in.pos;
        p_mid_POS.Dupsilon_1 = zeros(3,1);
        p_mid_POS.Dalpha_1 = zeros(3,1);
        p_mid_POS.Dupsilon = zeros(3,1);
        p_mid_POS.Dalpha = zeros(3,1);
    case int8(1)
        %% 姿态
        CLN = [ 0 1 0; 1 0 0; 0 0 -1 ];
        CNE_2 = dcmenu2ecef(p_mid_POS.pos_2(1,1), p_mid_POS.pos_2(2,1));
        earthPar_n2 = calcearthpar_pgs( CNE_2, p_mid_POS.pos_2(3,1), p_mid_POS.vN_2(1,1),  NavCoordType.GEO, par.useExtGravity, par.g, cst );
        CNE_1 = dcmenu2ecef(p_mid_POS.pos_1(1,1), p_mid_POS.pos_1(2,1));
        earthPar_n1 = calcearthpar_pgs( CNE_1, p_mid_POS.pos_1(3,1), p_mid_POS.vN_1(1,1),  NavCoordType.GEO, par.useExtGravity, par.g, cst );
        CNE = dcmenu2ecef(in.pos(1,1), in.pos(2,1));
        earthPar_n = calcearthpar_pgs( CNE, in.pos(3,1), in.vN(1,1),  NavCoordType.GEO, par.useExtGravity, par.g, cst );
        
        omegaIEN_nh = (1*earthPar_n.omegaIEN + 1*earthPar_n1.omegaIEN) / 2;
        rhoNz_nh = (1*earthPar_n.rhoNz + 1*earthPar_n1.rhoNz) / 2;
        FCN_nh = (1*earthPar_n.FCN + 1*earthPar_n1.FCN) / 2;
        % 与函数att_ls_dcm相对应
        if cfg.CLLType == 1
            DRN_n = (1*p_mid_POS.vN_1 + 1*in.vN) * par.dt/2;
            zeta_n = cst.CNL * (omegaIEN_nh*par.dt + rhoNz_nh*cst.uZNN*par.dt + FCN_nh*cross(cst.uZNN, DRN_n));
            CL_n1L_n = rv2dcm_5thod(-zeta_n');
        else
            CL_n1L_n = (CNE * CLN)' * (CNE_1 * CLN); % FIXME: 飞行轨迹逆推误差较大，应考虑omegaIE？
        end
        %
        CB_mB_m1 = (CL_n1L_n * p_mid_POS.CBL_1)^(-1) * in.CBL;
        CB_mB_m1 = dcmnormortho_order(CB_mB_m1);
        % 等效旋转矢量
        if cfg.rotVecType == 1
            qB_mB_m1 = dcm2quat_cg(CB_mB_m1');
            if qB_mB_m1(1) < 0
                qB_mB_m1(2:4) = -qB_mB_m1(2:4);
            end
            theta = 2 * atan(sqrt(qB_mB_m1(2)^2 + qB_mB_m1(3)^2 + qB_mB_m1(4)^2) / qB_mB_m1(1));
            theta_x = theta / sin(theta/2) * qB_mB_m1(2);
            theta_y = theta / sin(theta/2) * qB_mB_m1(3);
            theta_z = theta / sin(theta/2) * qB_mB_m1(4);
            phi_m = [ theta_x theta_y theta_z ]';
        else
            phi_m = dcm2rv(CB_mB_m1)';
        end
        %
        p_mid_POS.Dalpha = (eye(3) + 1/12*skewsymmat(p_mid_POS.Dalpha_1))^(-1) * (phi_m); % TODO: 初始采样计算方式与文档不一致
        
        %% 速度
        % 与函数vel_ns相对应
        vN_nh = (3*p_mid_POS.vN_1 - 1*p_mid_POS.vN_2) / 2;
        gN_nh = (3*earthPar_n1.gN - 1*earthPar_n2.gN) / 2;
        omegaIEN_n = (3*earthPar_n1.omegaIEN - 1*earthPar_n2.omegaIEN) / 2;
        rhoNz_n = (3*earthPar_n1.rhoNz - 1*earthPar_n2.rhoNz) / 2;
        FCN_n = (3*earthPar_n1.FCN - 1*earthPar_n2.FCN) / 2;
        
        DvGCorN_m = (gN_nh - cross(2*omegaIEN_n + rhoNz_n*cst.uZNN + FCN_n*cross(cst.uZNN, vN_nh), vN_nh)) * par.dt;
        DvSFL_m = cst.CNL*(in.vN - p_mid_POS.vN_1 - DvGCorN_m);
        
        DRN_m = (3*p_mid_POS.vN_1 - 1*p_mid_POS.vN_2) * par.dt/2;
        zeta_m = cst.CNL * (omegaIEN_n*par.dt + rhoNz_n*cst.uZNN*par.dt + FCN_n*cross(cst.uZNN, DRN_m));
        CL_n1L_m = eye(3) - skewsymmat(zeta_m);
        DvSFB_m1_m = 2 * ( (CL_n1L_m+eye(3))*p_mid_POS.CBL_1 )^(-1) * DvSFL_m;
        p_mid_POS.Dupsilon = (eye(3) + 1/12*skewsymmat(p_mid_POS.Dalpha_1) + 1/2*skewsymmat(p_mid_POS.Dalpha))^(-1) * (DvSFB_m1_m - 1/12*cross(p_mid_POS.Dupsilon_1,p_mid_POS.Dalpha)); % TODO: 初始采样计算方式与文档不一致
        %
        p_mid_POS.CBL_2 = p_mid_POS.CBL_1;
        p_mid_POS.vN_2 = p_mid_POS.vN_1;
        p_mid_POS.pos_2 = p_mid_POS.pos_1;
        p_mid_POS.CBL_1 = in.CBL;
        p_mid_POS.vN_1 = in.vN;
        p_mid_POS.pos_1 = in.pos;
        p_mid_POS.Dupsilon_1 = p_mid_POS.Dupsilon;
        p_mid_POS.Dalpha_1 = p_mid_POS.Dalpha;
        
    otherwise % 仅输出，不进行任何操作
end
Dupsilon = p_mid_POS.Dupsilon;
Dalpha = p_mid_POS.Dalpha;
fB = (p_mid_POS.Dupsilon + p_mid_POS.Dupsilon_1)/2/par.dt;
omegaIBB = (p_mid_POS.Dalpha + p_mid_POS.Dalpha_1)/2/par.dt;
end